#include <ros/ros.h>
#include <geometry_msgs/TransformStamped.h>
#include "include/navigation/navigation.h"
#include <tf2_msgs/TFMessage.h>
#include "include/serial/Serial.h"
#include "include/UART/UART.h"
#include <thread>
#include <iostream>
using namespace std;

//#define DEBUG_BY_KEYBOARD
//#define CTRL_DIRECT

//UART连接数传给飞控核心板
//Serial连接云台

void navigate2target(int datax,int datay);
void transformCallback(const tf2_msgs::TFMessage::ConstPtr& msg);
void spinThread();

int main(int argc, char** argv)
{
    ros::init(argc, argv, "transform_subscriber");
    ros::NodeHandle nh;
    
    
    ros::Subscriber sub = nh.subscribe<tf2_msgs::TFMessage>("/tf", 10, transformCallback);
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    Navigation::Get()->init(pub);
    thread(spinThread).detach();
#ifndef DEBUG_BY_KEYBOARD
    thread(Serial::Get()->SerialTask).detach();
    thread(UART::Get()->SerialTask).detach();
  #endif
    ros::spin();
    return 0;
}

void navigate2target(int datax,int datay)
{
    if(datay == 0)
    {
        if(datax == 0)
        {
          Navigation::Get()->TurnLeft(45);
          Navigation::Get()->MoveForward(0.95);
          Navigation::Get()->TurnRight(45);
          Navigation::Get()->MoveForward(0.5);
          Navigation::Get()->TurnRight(45);
        }
        else if(datax == 1)
        {
          Navigation::Get()->TurnRight(45);
          Navigation::Get()->MoveForward(0.15);
          Navigation::Get()->TurnLeft(45);
          Navigation::Get()->MoveForward(0.7);
          Navigation::Get()->TurnRight(45);
          Navigation::Get()->MoveForward(0.2);
        }
        else if(datax == 2)
        {
          Navigation::Get()->TurnRight(85);
          Navigation::Get()->MoveForward(1);
          Navigation::Get()->TurnLeft(90);
          Navigation::Get()->MoveForward(1.2);
          Navigation::Get()->TurnRight(40);
        }
        else if(datax == 3)
        {
          Navigation::Get()->MoveForward(0.65);
        }
        else if(datax == 4)
        {
          Navigation::Get()->MoveForward(0.05);
          Navigation::Get()->TurnRight(45);
          Navigation::Get()->MoveForward(0.5);
        }
        else if(datax == 5)
        {
          Navigation::Get()->TurnRight(45);
          Navigation::Get()->MoveForward(0.15);
          Navigation::Get()->TurnLeft(45);
          Navigation::Get()->MoveForward(0.8);
          Navigation::Get()->TurnRight(87);
          Navigation::Get()->MoveForward(1.1);
          Navigation::Get()->TurnRight(40);
        }
        else if(datax == 6)
        {
          Navigation::Get()->TurnRight(130);
          Navigation::Get()->TurnLeft(45);
          Navigation::Get()->MoveForward(0.9);
          Navigation::Get()->TurnLeft(45);
        }
    }
    else if(datay == 1)
    {
        if(datax == 0)
        {
          Navigation::Get()->BackRight(45);
          Navigation::Get()->MoveBackward(0.5);
          Navigation::Get()->BackRight(60);
          Navigation::Get()->MoveBackward(0.8);
          Navigation::Get()->BackLeft(60);
        }
        else if(datax == 1)
        {
          Navigation::Get()->MoveBackward(0.1);
          Navigation::Get()->BackRight(45);
          Navigation::Get()->MoveBackward(0.7);
          Navigation::Get()->BackLeft(45);
          Navigation::Get()->BackRight(45);
          Navigation::Get()->MoveBackward(0.15);
        }
        else if(datax == 2)
        {
          Navigation::Get()->BackRight(40);
          Navigation::Get()->MoveBackward(1.1);
          Navigation::Get()->BackLeft(90);
          Navigation::Get()->MoveBackward(0.9);
          Navigation::Get()->BackRight(90);
        }
        else if(datax == 3)
        {
          Navigation::Get()->MoveBackward(0.65);
        }
        else if(datax == 4)
        {
          Navigation::Get()->MoveBackward(0.5);
          Navigation::Get()->BackRight(45);
          Navigation::Get()->MoveBackward(0.05);
        }
        else if(datax == 5)
        {
          Navigation::Get()->BackRight(45);
          Navigation::Get()->MoveBackward(0.85);
          Navigation::Get()->BackRight(85);
          Navigation::Get()->MoveBackward(1.1);
          Navigation::Get()->BackLeft(45);
          Navigation::Get()->BackRight(45);
          Navigation::Get()->MoveBackward(0.15);
        }
        else if(datax == 6)
        {
          Navigation::Get()->BackLeft(45);
          Navigation::Get()->MoveBackward(0.8);
          Navigation::Get()->BackLeft(45);
          Navigation::Get()->BackRight(100);
        }
    }
}

void transformCallback(const tf2_msgs::TFMessage::ConstPtr& msg)
{
    for (const auto& transform : msg->transforms)
    {
        if (transform.header.frame_id == "map" && transform.child_frame_id == "base_laser_link")
        {
             Navigation::Get()->UpdatePosition(msg);
        }
    }
   
}

void spinThread()
{
    #ifndef DEBUG_BY_KEYBOARD
    bool fire_mission_complete = 0;
    while(1)
    {
      _ReceiveStruct flightCtrlData = UART::Get()->GetData();
      //cout << "x = " << flightCtrlData.dataX << "y = " << flightCtrlData.dataY << endl;
      if(flightCtrlData.cmd == 0)
      {
        ReceiveStruct arduinoData = Serial::Get()->GetData();
        if(arduinoData.find == 1)
          UART::Get()->SendData(1);
        Serial::Get()->SetTakeOffFlag(0);
      }
      else if(flightCtrlData.cmd == 1)
      {
        Serial::Get()->SetTakeOffFlag(1);
        delayMilliseconds(250);
        Serial::Get()->SendData(flightCtrlData.cmd,flightCtrlData.dataX,flightCtrlData.dataY);
      }
      else if(flightCtrlData.cmd == 2)
      {
        Serial::Get()->SetTakeOffFlag(1);
        Serial::Get()->SendData(flightCtrlData.cmd,flightCtrlData.dataX,flightCtrlData.dataY);
        if(fire_mission_complete)
          continue;
        int posion = -1;
        if(flightCtrlData.dataY > 23)
        {
          if(flightCtrlData.dataX < 20)
            posion = 0;
          else if(flightCtrlData.dataX > 20 && flightCtrlData.dataX < 34)
            posion = 1;
          else
            posion = 2;
        }
        else
        {
          if(flightCtrlData.dataX < 17)
            posion = 3;
          else if(flightCtrlData.dataX > 17 && flightCtrlData.dataX < 34)
            posion = 4;
          else
          {
            if(flightCtrlData.dataY >12)
              posion = 5;
            else
              posion = 6;
          }
        }
        delayMilliseconds(250);
        Serial::Get()->SendData(flightCtrlData.cmd,flightCtrlData.dataX,flightCtrlData.dataY);
        navigate2target(posion,0);
        int loopcount = 0;
        while(1)
        {
          loopcount += 1;
          Serial::Get()->SendData(flightCtrlData.cmd,flightCtrlData.dataX,flightCtrlData.dataY);
          delayMilliseconds(1000);
          if(loopcount == 60)
          {
            break;
          }
        }
        navigate2target(posion,1);
        fire_mission_complete = 1;
      }
    }
  #else
    #ifdef CTRL_DIRECT
    while(1)
    {
      cout << "请输入指令：";
      string a;
      cin >> a;
      cout << "\n请输入距离或角度:";
      float data;
      cin >> data;
      if(a == "w")
      {
         Navigation::Get()->MoveForward(data);
      }
      else if(a == "a")
      {
        Navigation::Get()->TurnLeft(data);
      }
      else if(a == "s")
      {
        Navigation::Get()->MoveBackward(data);
      }
      else if(a == "d")
      {
        Navigation::Get()->TurnRight(data);
      }
      else if(a == "as" || a == "sa")
      {
        Navigation::Get()->BackLeft(data);
      }
      else if(a == "sd" || a == "ds")
      {
        Navigation::Get()->BackRight(data);
      }
    }
    #else
    while(1)
    {
      cout << "请出入目标点：";
      int target = -1;
      cin >> target;
      navigate2target(target,0);
      cout << "导航完成\n输入任何值返回出发点";
      string a;
      cin >> a;
      navigate2target(target,1);
    }
    #endif
  #endif
}